/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.Timer;
import java.util.Random;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    //Joysticks
    Joystick joystickLeft = new Joystick(1);
    Joystick joystickRight = new Joystick(2);
    Joystick joystickControl = new Joystick(3);
    //CAN Jaguars
    CANJaguar LDrive;
    CANJaguar RDrive;
    CANJaguar shooter;
    //Victors
    Victor roller1 = new Victor(1);
    Victor roller2 = new Victor(2);
    Victor trigger = new Victor(3);
    Victor arm = new Victor(4);
    //Spike
    Relay intake = new Relay(5);
    //Limit Switches
    DigitalInput armLimitDown = new DigitalInput(7);
    DigitalInput armLimitUp = new DigitalInput(8);
    //Encoders
    Encoder RDriveEncoder = new Encoder(1, 2);
    Encoder LDriveEncoder = new Encoder(3, 4);
    Encoder shooterEncoder = new Encoder(5, 6);
    //Right Joystick Buttons
    JoystickButton singleJoystickDriveButton = new JoystickButton(joystickRight, 1);
    JoystickButton PIDTurnButton = new JoystickButton(joystickRight, 2);
    JoystickButton manualTurnButton = new JoystickButton(joystickRight, 4);
    JoystickButton armButton = new JoystickButton(joystickRight, 3);
    //Left Joystick Buttons
    JoystickButton ballIntakeButton = new JoystickButton(joystickLeft, 1);
    JoystickButton brakeButton = new JoystickButton(joystickLeft, 2);
    //Control Joystick Buttons
    JoystickButton shootBallButton = new JoystickButton(joystickControl, 1);
    JoystickButton prepShooterButton = new JoystickButton(joystickControl, 3);
    JoystickButton dashboardDisplayButton = new JoystickButton(joystickControl, 2);//Ony for debugging
    //Drive
    RobotDrive mainDrive;
    //Timers
    Timer shooterTimer = new Timer();
    Timer shooterPIDTimer = new Timer();
    Timer autonomousTimer = new Timer();
    //Booleans
    boolean brakeSet = false;
    boolean shooterOn = false;
    boolean shooterSpeedOK = false;
    boolean fireBall = false;
    boolean armGoingDown = false;
    boolean armGoingUp = false;
    boolean bridgeDown = false;
    //Random
    Random autonomous = new Random();
    //PID Variables
    //Doubles:
    double leftEncoderVal, rightEncoderVal;
    double Lspeed = 0;
    double Rspeed = 0;
    double setLspeed, setRspeed; //speed setpoints
    double Kp, Kpl, Kpr, Kil, Kir, Kdl, Kdr; //Kx constants for left & right drives
    double Kps, Kis, Kds; //Kx constants for shooter
    double lPIDerrorInt = 0; // used for Integral PID control
    double rPIDerrorInt = 0;
    double sPIDerrorInt = 0;
    double previousLerror, previousRerror, previousSerror;// used for Differential PID control
    double lEncoder, rEncoder;
    double ml1,ml2,mr1,mr2;
    double oldPIDint, maxPIDint, minPIDint;
    double sSpeed = 0;
    double setSspeed;
    double ms;
    double oldsPIDint;
    //Integers
    int mZeroCnt=0, mNZCnt=0; //motor Zero value counter
    //Booleans:
    boolean error = false;
    boolean initialSpeedReached = false;
    //E.o. PID Variables

    public void printToClassmate(){
            DriverStationLCD driverStation = DriverStationLCD.getInstance();
            //driverStation.println(DriverStationLCD.Line.kMain6, 1, "mZeroCnt: " + mZeroCnt + "                          ");
            //driverStation.println(DriverStationLCD.Line.kUser2, 1, "mNZCnt: "  + mNZCnt  + "                          ");
            //driverStation.println(DriverStationLCD.Line.kUser3, 1, "leftEncoderVal: " + (Math.floor(leftEncoderVal*1)/1) + "                          ");
            //driverStation.println(DriverStationLCD.Line.kUser4, 1, "rightEncoderVal: " + (Math.floor(rightEncoderVal*1)/1)  + "                          ");
            //driverStation.println(DriverStationLCD.Line.kUser5, 1, "ml1: " + (Math.floor(ml1*100)/100) + "                          ");
            //driverStation.println(DriverStationLCD.Line.kUser6, 1, "mr1: " + (Math.floor(mr1*100)/100) + "                          ");
            driverStation.updateLCD();
    }
    public void updateDashboard() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 1;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 2;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            lowDashData.addByte(Solenoid.getAllFromDefaultModule());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

    }
    public void robotInit(){
        try{
            RDrive = new CANJaguar(9, CANJaguar.ControlMode.kPercentVbus);
            LDrive = new CANJaguar(10, CANJaguar.ControlMode.kPercentVbus);
            shooter = new CANJaguar(8, CANJaguar.ControlMode.kPercentVbus);
        }
        catch( CANTimeoutException ex ){
            ex.printStackTrace();
        }
        mainDrive = new RobotDrive(RDrive, LDrive);
        brakeSetFalse();
    }
    public void normalDrive(){
        mainDrive.tankDrive(joystickRight, joystickLeft);
    }
    public void singleJoystickDrive(){
        mainDrive.tankDrive(joystickLeft, joystickLeft);
    }
    public void PIDDrive(int d){
        //Main PID drive L, R, REV, FWD
        /* Values of d:
         * 1 Left Turn
         * 2 Right turn
         * 3 Reverse
         * 4 Forward
         */
        final double MAX_MOTOR_POWER = 0.5;//Cap the max value to avoid erratic trashing
        int leftFWD =1 ;
        int rightFWD =1 ;
        // Init stuff for testPID
        //setLspeed=(leftStick.getRawAxis(4)+1)*5000+1; //SetPoint
        setLspeed=(joystickLeft.getRawAxis(4)+1)*1000+1; //SetPoint

        //setLspeed = 900;
        setRspeed=setLspeed;

        Kp= 0.25 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise Kp.
        Kil= 0.05 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise K.
        Kir= 0.05 * 1/setRspeed;
        Kdl= 0.00 * 1/setLspeed;
        Kdr= 0.00 * 1/setRspeed;
        //setLspeed = setLspeed * 2;
        // eo Init stuff for testPID

        if(d == 1){//Left Turn
            leftFWD = -1;
            rightFWD = 1;
        }
        if(d == 2){//Right Turn
            leftFWD = 1;
            rightFWD = -1;
        }
        if(d == 3){//Reverse
            leftFWD = -1;
            rightFWD = -1;
        }
        if(d == 4){//Forward
            leftFWD = 1;
            rightFWD = 1;
        }
        leftEncoderVal=LDriveEncoder.getRate()* leftFWD;
        rightEncoderVal=RDriveEncoder.getRate() * rightFWD * -1;

        if(Double.isNaN(Lspeed)){ //for some reason the encoder object returns NaN, so just overwrite it!!!
            Lspeed = 0;
            error = true;
        }
        if(Double.isNaN(Rspeed)){
            Rspeed = 0;
            error = true;
        }

        //Lspeed=(Lspeed + leftEncoderVal)/2; // this is done to smooth getRate as it is naturally erratic.
        //Rspeed=(Rspeed + rightEncoderVal)/2;
        Lspeed=leftEncoderVal;// it seems more unstable without the averaging above
        Rspeed=rightEncoderVal;

        ml1=Kp*(setLspeed-Lspeed)+Kil*lPIDerrorInt+Kdl*((setLspeed-Lspeed)-previousLerror);
        mr1=Kp*(setRspeed-Rspeed)+Kir*rPIDerrorInt+Kdr*((setRspeed-Rspeed)-previousRerror);
        if (ml1>MAX_MOTOR_POWER) ml1=MAX_MOTOR_POWER;// cap the max value to avoid erratic trashing
        if (mr1>MAX_MOTOR_POWER) mr1=MAX_MOTOR_POWER;
        ml2=ml1;
        mr2=mr1;

        LDrive.set(ml1 * leftFWD * -1);
        RDrive.set(mr1 * rightFWD);

        oldPIDint=lPIDerrorInt;
        lPIDerrorInt=lPIDerrorInt + (setLspeed-Lspeed); //compute integral of PID error
        rPIDerrorInt=rPIDerrorInt + (setRspeed-Rspeed); //compute integral of PID error
        if (lPIDerrorInt>maxPIDint) maxPIDint=lPIDerrorInt;
        if (lPIDerrorInt<minPIDint) minPIDint=lPIDerrorInt;

        previousLerror=setLspeed-Lspeed;// differential PID
        previousRerror=setRspeed-Rspeed;

        if (ml1==0) mZeroCnt++; else mNZCnt++;
    }
    public void manualTurn(){
        mainDrive.tankDrive(-joystickRight.getRawAxis(3) / 1.7, joystickRight.getRawAxis(3) / 1.7);
    }
    public void ballIntakeOn(){
        intake.set(Relay.Value.kOn);
        roller1.set(0.5);
        roller2.set(0.5);
    }
    public void ballIntakeOff(){
        intake.set(Relay.Value.kOff);
        roller1.set(0);
        roller2.set(0);
    }
    public void brakeSetTrue(){
        try{
            LDrive.configNeutralMode(CANJaguar.NeutralMode.kBrake);
            RDrive.configNeutralMode(CANJaguar.NeutralMode.kBrake);
        }
        catch(CANTimeoutException ex){
            ex.printStackTrace();
        }
    }
    public void brakeSetFalse(){
        try{
            LDrive.configNeutralMode(CANJaguar.NeutralMode.kCoast);
            RDrive.configNeutralMode(CANJaguar.NeutralMode.kCoast);
        }
        catch(CANTimeoutException ex){
            ex.printStackTrace();
        }
    }
    public void shooterPID(){
        double encoderVal;

        // Init stuff for testPID
        setSspeed=(joystickLeft.getRawAxis(4)+1)*5000; //SetPoint
        //setSspeed = setSspeed * 2;
        // eo Init stuff for testPID
        Kps= 0.25 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise Kp.
        Kis= 0.05 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise K.
        Kds= 0.00 * 1/setLspeed;

         //this code can be inside the "porous" finemotion when a button is pressed.
        encoderVal=shooterEncoder.getRate()*(-1);  // this is done to smooth getRate as it is naturally erratic.
        sSpeed=(sSpeed + encoderVal)/2;

        ms=Kp*(setSspeed-sSpeed)+Kil*sPIDerrorInt+Kdl*((setSspeed-sSpeed)-previousSerror);
        // The P in PID:

        LDrive.set(ms);

        oldsPIDint=sPIDerrorInt;
        sPIDerrorInt=sPIDerrorInt + (setSspeed-sSpeed); //compute integral of PID error

        if((encoderVal > setSspeed) && (initialSpeedReached == false)){
            shooterPIDTimer.reset();
            initialSpeedReached = true;
        }
        if(shooterPIDTimer.get() > 2){
            shooterSpeedOK = true;
        }
    }
    public void shootBall(){
        if(shooterSpeedOK == true){
            if(shooterTimer.get() < 10){
                trigger.set(0.8);
            }
            else{
                trigger.set(0);
                fireBall = false;
                shooterOn = false;
                shooterSpeedOK = false;
                initialSpeedReached = false;
            }
        }
    }
    public void armDown(){
        if(armGoingDown == false){
            armGoingDown = true;
        }
        if((armGoingDown == true) && (armLimitDown.get() == false) && (bridgeDown == false)){
            arm.set(0.5);
        }
        if((armGoingDown == true) && (armLimitDown.get() == true)){
            arm.set(0);
            bridgeDown = true;
        }
    }
    public void armUp(){
        if(armLimitUp.get() == false){
            arm.set(-0.5);
        }
        if(armLimitUp.get() == true){
            arm.set(0);
        }
        bridgeDown = false;
    }
    public void autonomous(){
        //Random delay, shoot first ball
        //Random delay, shoot second ball
        double delay = autonomous.nextDouble() * 5;
        autonomousTimer.reset();

        while(autonomousTimer.get() < delay){

        }
        shooterOn = true;
        while(shooterOn == true){
            shooterPID();
            fireBall = true;
            shootBall();
        }
        shooterOn = true;
        while(shooterOn == true){
            shooterPID();
            fireBall = true;
            shootBall();
        }
    }
    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
         while (isOperatorControl() && isEnabled()){
            if(singleJoystickDriveButton.get() == true){
                singleJoystickDrive();
            }
            if(joystickRight.getRawAxis(6) > 0.5){// REV PID
                PIDDrive(3);
            }
            if(joystickRight.getRawAxis(6) < -0.5){// FWD PID
                PIDDrive(4);
            }
            if(PIDTurnButton.get() == true){
                if (joystickRight.getRawAxis(3) > 0.5) PIDDrive(2);// Right turn PID
                if (joystickRight.getRawAxis(3) < -0.5) PIDDrive(1);// Left turn PID
            }
            if(manualTurnButton.get() == true){
                manualTurn();
            }
            if(ballIntakeButton.get() == true){
                ballIntakeOn();
            }
            else{
                ballIntakeOff();
            }
            if(brakeButton.get() == true){
                if(brakeSet == false){
                    brakeSetTrue();
                    brakeSet = true;
                }
                else{
                if(brakeSet == true ){
                        brakeSetFalse();
                        brakeSet = false;
                    }
                }
            }
            if(prepShooterButton.get() == true){
                shooterOn = true;
            }
            if(shootBallButton.get() == true){
                if(fireBall == false){
                    shooterTimer.reset();
                    shooterOn = true;
                    fireBall = true;
                }
            }
            if(shooterOn == true){
                shooterPID();
            }
            if(fireBall == true){
                shootBall();
            }
            if(armButton.get() == true){
                armDown();
            }
            else{
                armUp();
            }
            if(dashboardDisplayButton.get() == true){
                updateDashboard();
            }
         }
    }
}
/*
To add:
 * Find power needed for arm
 * Find power needed for PID values
 * Messages for Classmate in the game: Indicate arm limit switch is activated
 */